import rclpy
from rclpy.node import Node


from sls_controller.sls_uavResource import UAVResource
from sls_controller.sls_avResource import AVResource
from sls_controller.sls_publicDefine import *

import json



#change it when you want to change the vehicle type
vehicleClass = VehicleClass.AV
vehicleID = "fly1"
class MainController(Node):
    def __init__(self):
        super().__init__('mainController_node')
        self.first_enter_flag = True
        self.vehicleClass = vehicleClass
        self.vehicleID = vehicleID
        #根据需要调整执行频率
        self.main_timer_during = ParamDefault.MAIN_TIMER_DURING
        #！！！考虑异步方式执行
        self.main_timer = self.create_timer(self.main_timer_during,self.main_timer_callback)
        ##车辆资源初始化
        if self.vehicleClass == VehicleClass.UAV:
            #UAV的初始化
            ##UAV的资源镜像初始化
            pass
        elif self.vehicleClass == VehicleClass.AV:
            #AV的初始化
            ##AV的资源镜像初始化
            self.vehicleResource = AVResource(main_node = self, idStr = self.vehicleID)
        #公共资源初始化

    def main_timer_callback(self) -> None:
        if self.first_enter_flag == True:
            self.first_enter_callback()
            self.first_enter_flag = False
        start_time = float(self.get_clock().now().nanoseconds / 1000000.0)
        if self.vehicleClass == VehicleClass.UAV:
            pass
        elif self.vehicleClass == VehicleClass.AV:
            #逻辑
            self.main_task()
        print('main_process durafirst_enter_callbacktion time is '+str(float(self.get_clock().now().nanoseconds / 1000000.0)-start_time)+' ms')


    #第一次运行要做的事
    def first_enter_callback(self):  
        if self.vehicleClass == VehicleClass.UAV:
            pass
        elif self.vehicleClass == VehicleClass.AV:
            pass
        #一般不要直接开始任务，建议通过上位机开始任务
        #self.missionsSchedule.startMissions()

    def main_task(self):
        #在这里写测试,看看self.vehicleResource里面的东西是不是都能用
        pass

def main(args=None) -> None:
    print('Starting offboard control node...')
    rclpy.init(args=args)
    mainController = MainController()
    #利用多线程来避免回调函数的阻塞（定时回调，发布和订阅回调）
    executor = MultiThreadedExecutor()
    executor.add_node(mainController)
    executor.spin()
    #rclpy.spin(mainController)
    mainController.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    try:
        main()
    except Exception as e:
        print(e)